www.gusucode.com > 粒子滤波全套源代码,实现了粒子滤波的全部功能,是粒子滤波提高级程序 > particle filter program/Observe_and_Estimate.m
function [Sample_probability,Estimate,vx,vy,after_prop]=Observe_and_Estimate(Sample_Set,Estimate,Hx,Hy,target_histgram,new_sita,loop,after_prop,I,N) Total_probability=0; for i=1:1:N Sample_histgram(i).element=histgram((Sample_Set(i).x),(Sample_Set(i).y),Hx,Hy,I); Sample_probability(i)=weight(target_histgram,Sample_histgram(i).element,new_sita); Total_probability=Total_probability+Sample_probability(i); end Total_probability Estimate(loop).x=0; Estimate(loop).y=0; for i=1:1:N Estimate(loop).x=Estimate(loop).x+double(Sample_Set(i).x)*(Sample_probability(i)/Total_probability); Estimate(loop).y=Estimate(loop).y+double(Sample_Set(i).y)*(Sample_probability(i)/Total_probability); end Estimate(loop).histgram=histgram(round(Estimate(loop).x),round(Estimate(loop).y),Hx,Hy,I); Estimate(loop).prob=weight(target_histgram,Estimate(loop).histgram,new_sita); %计算速度 a=floor(Estimate(loop).x); b=floor(Estimate(loop-1).x); vx=a-b; c=floor(Estimate(loop).y); d=floor(Estimate(loop-1).y); vy=c-d; i=1; for new_x=a-i:a+i; for new_y=c-i:c+i; after_prop(new_y,new_x,:)=255; end; end; % edge_x=a-Hx; % edge_y=c-Hy; % for i=1:2*Hx % after_prop(c-Hy,edge_x+i,:)=255; % after_prop(c+Hy,edge_x+i,:)=255; % end; % for i=1:2*Hy % after_prop(edge_y+i,a-Hx,:)=255; % after_prop(edge_y+i,a+Hx,:)=255; % end; a=num2str(loop,'%03.3g'); b=['d:\result\',a,'.jpg']; imwrite(after_prop,b); %imshow([b]);